{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "id": "9ed9eb03-31f0-42ff-8213-324dd47a6ec0",
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import scipy as sp"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "id": "8f48fc07-f3ef-474f-a171-62ad9c39f8e5",
   "metadata": {},
   "outputs": [],
   "source": [
    "def rodrigez(fi,p):\n",
    "    e=np.eye(3)\n",
    "    px=np.array([[0,-p[2],p[1]],\n",
    "                 [p[2],0,-p[0]],\n",
    "                 [-p[1],p[0],0]])\n",
    "    ppT=np.array([[p[0]**2,p[0]*p[1],p[0]*p[2]]\n",
    "                 ,[p[1]*p[0],p[1]**2,p[1]*p[2]]\n",
    "                 ,[p[2]*p[0],p[2]*p[1],p[2]**2]])\n",
    "    return (1-np.cos(fi))*ppT+np.cos(fi)*e+np.sin(fi)*px"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 180,
   "id": "7c8edda5-33a0-4587-9f63-1ef141f3cd66",
   "metadata": {},
   "outputs": [],
   "source": [
    "def Euler2A(uglovi):\n",
    "    A=rodrigez(uglovi[0],[0,0,1])@rodrigez(uglovi[1],[0,1,0])@rodrigez(uglovi[2],[1,0,0])\n",
    "    A = np.where(np.isclose(A, 0) , 0 , A)  # da bi izbegli -0. u rezultatu\n",
    "    return A"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 179,
   "id": "61f27ddd-4067-4fbf-9892-0031f6b5148c",
   "metadata": {},
   "outputs": [],
   "source": [
    "def AxisAngle2A(pphi):\n",
    "    A=rodrigez(pphi[3],[pphi[0],pphi[1],pphi[2]])\n",
    "    A = np.where(np.isclose(A, 0) , 0.0 , A)  # izbegavanje -0. u rezultatu\n",
    "    return A"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "id": "3f3d59cd-29b5-44c3-a524-5f557e167812",
   "metadata": {},
   "outputs": [],
   "source": [
    "def nadji_vektor(sopstveni_vektori):\n",
    "    for i in range(len(sopstveni_vektori)):\n",
    "        j=0;\n",
    "        for elem in sopstveni_vektori[:,i]:\n",
    "            if elem.imag != 0:\n",
    "                break\n",
    "            j+=1;\n",
    "        if j==3:\n",
    "            return sopstveni_vektori[:,i]\n",
    "    raise Exception(\"Nema odgovarajuceg vektora!\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 178,
   "id": "678c50eb-f537-46c9-8e2c-c24f7a553abd",
   "metadata": {},
   "outputs": [],
   "source": [
    "def A2AxisAngle(A):\n",
    "# 1. Odredi jedinicni sopstveni vektor p za lambda=1\n",
    "# 2. Odredi proizvoljan jedinicni vektor u normalan na p\n",
    "# 3. Odredi u_prim koji je jednak A*u (u_prim je jedinicni)\n",
    "# 4. Odredi fi koji je jednak arccos(u,u_prim)\n",
    "# 5. Ako je mesoviti proizvod u,u_prim,p < 0, onda p=-p\n",
    "    provera=A@A.T\n",
    "    provera=np.where(np.isclose(provera,0),0,provera)\n",
    "    provera=np.where(np.isclose(provera,1),1,provera)\n",
    "    if (A.shape[0]!=A.shape[1]) or (not np.isclose(np.linalg.det(A),1)) or not np.all(provera==np.identity(A.shape[0])):\n",
    "        return \"Nije matrica kretanja!\"\n",
    "    if np.array_equal(A,np.eye(3)):\n",
    "        return np.array([1,0,0]),0\n",
    "# 1.\n",
    "    _,sopstveni_vektori=sp.linalg.eig(A,left=True,right=False) # Mora da se importuje scipy.linalg umesto samo scipy\n",
    "    # p=sopstveni_vektori[:,2] # Problem: Rucno biram odgovarajuci sopstveni vektor, jer nije tacno resenje sa svakim!\n",
    "    p=nadji_vektor(sopstveni_vektori)\n",
    "    #p=np.array(p)\n",
    "    p=np.real(p)\n",
    "# 2. Gram Šmit ortogonalizacija\n",
    "    u = np.array([1.0,2.0,3.0])\n",
    "    u -= u.dot(p) * p / np.linalg.norm(p)**2 \n",
    "    u /= np.linalg.norm(u)\n",
    "# 3.\n",
    "    u_prim=A@u\n",
    "    u_prim/=np.linalg.norm(u_prim)\n",
    "# 4.    \n",
    "    fi=np.arccos(u.dot(u_prim)/(np.linalg.norm(u)*np.linalg.norm(u_prim)))\n",
    "# 5.\n",
    "    if np.dot(u,np.cross(u_prim,p)) < 0:\n",
    "        p=-p\n",
    "    #return [p[0],p[1],p[2],fi]\n",
    "    pphi = np.array([p[0],p[1],p[2],fi])  # osa i ugao idu u jedan vektor\n",
    "    pphi = np.where(np.isclose(pphi, 0) , 0 , pphi)  # izbegavanje -0. u rezultatu\n",
    "    return pphi"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 181,
   "id": "7dbcec6b-0b46-4b91-9229-42c469ad6e55",
   "metadata": {
    "editable": true,
    "slideshow": {
     "slide_type": ""
    },
    "tags": []
   },
   "outputs": [],
   "source": [
    "def A2Euler(A):\n",
    "    uglovi=np.array([0,0,0]) # Zbog testera zadatka\n",
    "    provera=A@A.T\n",
    "    provera=np.where(np.isclose(provera,0),0,provera)\n",
    "    provera=np.where(np.isclose(provera,1),1,provera)\n",
    "    if (A.shape[0]!=A.shape[1]) or (not np.isclose(np.linalg.det(A),1)) or not np.all(provera==np.identity(A.shape[0])):\n",
    "        return \"Nije matrica kretanja!\"\n",
    "    if A[2][0] < 1:\n",
    "        if A[2][0] > -1: #jedinstveno resenje\n",
    "            psi=np.arctan2(A[1][0],A[0][0])\n",
    "            teta=np.arcsin(-A[2][0])\n",
    "            fi=np.arctan2(A[2][1],A[2][2])\n",
    "            uglovi=np.array([psi,teta,fi]) #\n",
    "            #return np.array([psi,teta,fi])\n",
    "        else: # Ox3=-Oz\n",
    "            psi=np.arctan2(-A[0][1],A[1][1])\n",
    "            teta=np.pi/2\n",
    "            fi=0\n",
    "            #return np.array([psi,teta,fi])\n",
    "            uglovi=np.array([psi,teta,fi]) #\n",
    "    else: #Ox3=Oz\n",
    "        psi=np.arctan2(-A[0][1],A[1][1])\n",
    "        teta=-np.pi/2\n",
    "        fi=0\n",
    "        #return np.array([psi,teta,fi])\n",
    "        uglovi=np.array([psi,teta,fi]) #\n",
    "    uglovi = np.where(np.isclose(uglovi, 0) , 0 , uglovi) # Isto  \n",
    "    return uglovi #"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "dc203157-e0a3-46f9-92f5-be58bcd278e0",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.18"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
